#include "dmMotor.h"
#include "djiMotor.h"

uint8_t Data_Enable[8]={0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};		//电机使能命令
uint8_t Data_Failure[8]={0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};		//电机失能命令
uint8_t Data_Save_zero[8]={0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE};	//电机保存零点命令

extern CAN_RxHeaderTypeDef rx_header;
extern uint8_t rx_data[8];

static CAN_TxHeaderTypeDef  tx_meessage;
static uint8_t              can_send_data[8];

Joint_Motor_t DM_motor[2];

float Hex_To_Float(uint32_t *Byte,int num)//十六进制到浮点数
{
	return *((float*)Byte);
}

uint32_t FloatTohex(float HEX)//浮点数到十六进制转换
{
	return *( uint32_t *)&HEX;
}

/**
************************************************************************
* @brief:      	float_to_uint: 浮点数转换为无符号整数函数
* @param[in]:   x_float:	待转换的浮点数
* @param[in]:   x_min:		范围最小值
* @param[in]:   x_max:		范围最大值
* @param[in]:   bits: 		目标无符号整数的位数
* @retval:     	无符号整数结果
* @details:    	将给定的浮点数 x 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个指定位数的无符号整数
************************************************************************
**/
int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
	/* Converts a float to an unsigned int, given range and number of bits */
	float span = x_max - x_min;
	float offset = x_min;
	return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
/**
************************************************************************
* @brief:      	uint_to_float: 无符号整数转换为浮点数函数
* @param[in]:   x_int: 待转换的无符号整数
* @param[in]:   x_min: 范围最小值
* @param[in]:   x_max: 范围最大值
* @param[in]:   bits:  无符号整数的位数
* @retval:     	浮点数结果
* @details:    	将给定的无符号整数 x_int 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个浮点数
************************************************************************
**/
float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
	/* converts unsigned int to float, given range and number of bits */
	float span = x_max - x_min;
	float offset = x_min;
	return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}

//在task里用
void joint_motor_init(Joint_Motor_t *motor,uint16_t id,uint16_t mode)
{
  motor->mode=mode;
  motor->para.id=id;
}

void wheel_motor_init(Wheel_Motor_t *motor,uint16_t id,uint16_t mode)
{
  motor->mode=mode;
  motor->para.id=id;
}

/**
************************************************************************
* @brief:      	dm4310_fbdata: 获取DM4310电机反馈数据函数
* @param[in]:   motor:    指向motor_t结构的指针，包含电机相关信息和反馈数据
* @param[in]:   rx_data:  指向包含反馈数据的数组指针
* @param[in]:   data_len: 数据长度
* @retval:     	void
* @details:    	从接收到的数据中提取DM4310电机的反馈信息，包括电机ID、
*               状态、位置、速度、扭矩相关温度参数、寄存器数据等
************************************************************************
**/
void dm4310_fbdata(Joint_Motor_t *motor, uint8_t *rx_data)
{ 

	//返回的数据有8个字节
	  motor->para.id = (rx_data[0])&0x0F;
	  motor->para.state = (rx_data[0])>>4;
	  motor->para.p_int=(rx_data[1]<<8)|rx_data[2];
	  motor->para.v_int=(rx_data[3]<<4)|(rx_data[4]>>4);
	  motor->para.t_int=((rx_data[4]&0xF)<<8)|rx_data[5];
	  motor->para.pos = uint_to_float(motor->para.p_int, P_MIN, P_MAX, 16); // (-12.5,12.5)
	  motor->para.vel = uint_to_float(motor->para.v_int, V_MIN, V_MAX, 12); // (-30.0,30.0)
	  motor->para.tor = uint_to_float(motor->para.t_int, T_MIN, T_MAX, 12);  // (-10.0,10.0)
	  motor->para.Tmos = (float)(rx_data[6]);
	  motor->para.Tcoil = (float)(rx_data[7]);
	
}


void dm6215_fbdata(Wheel_Motor_t *motor, uint8_t *rx_data)
{ 

	  motor->para.id = (rx_data[0])&0x0F;
	  motor->para.state = (rx_data[0])>>4;
	  motor->para.p_int=(rx_data[1]<<8)|rx_data[2];
	  motor->para.v_int=(rx_data[3]<<4)|(rx_data[4]>>4);
	  motor->para.t_int=((rx_data[4]&0xF)<<8)|rx_data[5];
	  motor->para.pos = uint_to_float(motor->para.p_int, P_MIN2, P_MAX2, 16); // (-12.0,12.0)
	  motor->para.vel = uint_to_float(motor->para.v_int, V_MIN2, V_MAX2, 12); // (-30.0,30.0)
	  motor->para.tor = uint_to_float(motor->para.t_int, T_MIN2, T_MAX2, 12);  // (-18.0,18.0)
	  motor->para.Tmos = (float)(rx_data[6]);
	  motor->para.Tcoil = (float)(rx_data[7]);

}

void DM_MotorEncode(void)
{

	switch (rx_header.StdId)
	{
	case 0x01:
	 dm4310_fbdata(&DM_motor[0],rx_data);
		
	break;
	
	case 0x02:
	dm4310_fbdata(&DM_motor[1],rx_data);
	   
    break;

	default:
		break;
	}

}

//初始化的时候
//mit 0x000 位置 0x100 速度 0x200

void enable_motor_mode(CAN_HandleTypeDef*hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint32_t send_mail_box;
	
	tx_meessage.StdId=motor_id+mode_id;
	tx_meessage.ExtId=0;
	tx_meessage.IDE=0;
	tx_meessage.RTR=0;
	tx_meessage.DLC=8;

	can_send_data[0] = 0xFF;
	can_send_data[1] = 0xFF;
	can_send_data[2] = 0xFF;
	can_send_data[3] = 0xFF;
	can_send_data[4] = 0xFF;
	can_send_data[5] = 0xFF;
	can_send_data[6] = 0xFF;
	can_send_data[7] = 0xFC;
	HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, &send_mail_box);

}


/**
************************************************************************
* @brief:      	disable_motor_mode: 禁用电机模式函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要禁用的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送禁用特定模式的命令
************************************************************************
**/
void disable_motor_mode(CAN_HandleTypeDef*hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint32_t send_mail_box;
	
	tx_meessage.StdId=motor_id;
	tx_meessage.ExtId=0;
	tx_meessage.IDE=0;
	tx_meessage.RTR=0;
	tx_meessage.DLC=8;
	
	can_send_data[0] = 0xFF;
	can_send_data[1] = 0xFF;
	can_send_data[2] = 0xFF;
	can_send_data[3] = 0xFF;
	can_send_data[4] = 0xFF;
	can_send_data[5] = 0xFF;
	can_send_data[6] = 0xFF;
	can_send_data[7] = 0xFD;
	
	HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, &send_mail_box);
}

/**
************************************************************************
* @brief:      	mit_ctrl: MIT模式下的电机控制函数
* @param[in]:   hcan:			指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id:	电机ID，指定目标电机
* @param[in]:   pos:			位置给定值
* @param[in]:   vel:			速度给定值
* @param[in]:   kp:				位置比例系数
* @param[in]:   kd:				位置微分系数
* @param[in]:   torq:			转矩给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送MIT模式下的控制帧。
************************************************************************
**/
void mit_ctrl(CAN_HandleTypeDef*hcan, uint16_t motor_id, float pos, float vel,float kp, float kd, float torq)
{

	uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;

	pos_tmp = float_to_uint(pos,  P_MIN,  P_MAX,  16);
	vel_tmp = float_to_uint(vel,  V_MIN,  V_MAX,  12);
	kp_tmp  = float_to_uint(kp,   KP_MIN, KP_MAX, 12);
	kd_tmp  = float_to_uint(kd,   KD_MIN, KD_MAX, 12);
	tor_tmp = float_to_uint(torq, T_MIN,  T_MAX,  12);

	tx_meessage.StdId=motor_id;
	tx_meessage.IDE=CAN_ID_STD;
	tx_meessage.RTR=CAN_RTR_DATA;
	tx_meessage.DLC=0x08;

	can_send_data[0] = (pos_tmp >> 8);
	can_send_data[1] = pos_tmp;
	can_send_data[2] = (vel_tmp >> 4);
	can_send_data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
	can_send_data[4] = kp_tmp;
	can_send_data[5] = (kd_tmp >> 4);
	can_send_data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
	can_send_data[7] = tor_tmp;
	
	//寻空邮箱发送数据
	if(HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) 
	{
		if(HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX1) != HAL_OK)
		{
			HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX2);
		}
    }

}
/**
************************************************************************
* @brief:      	pos_speed_ctrl: 位置速度控制函数
* @param[in]:   hcan:			指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id:	电机ID，指定目标电机
* @param[in]:   vel:			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送位置速度控制命令
************************************************************************
**/
void pos_speed_ctrl(CAN_HandleTypeDef*hcan,uint16_t motor_id, float pos, float vel)
{

	uint8_t *pbuf, *vbuf;
	pbuf=(uint8_t*)&pos;
	vbuf=(uint8_t*)&vel;
	
	tx_meessage.StdId=motor_id;
	tx_meessage.IDE=CAN_ID_STD;
	tx_meessage.RTR=CAN_RTR_DATA;
	tx_meessage.DLC=0x08;

	can_send_data[0] = *pbuf;
	can_send_data[1] = *(pbuf+1);
	can_send_data[2] = *(pbuf+2);
	can_send_data[3] = *(pbuf+3);

	can_send_data[4] = *vbuf;
	can_send_data[5] = *(vbuf+1);
	can_send_data[6] = *(vbuf+2);
	can_send_data[7] = *(vbuf+3);
	
	//寻空邮箱发送数据
	if(HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) 
	{
		if(HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX1) != HAL_OK)
		{
			HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX2);
		}
	}

}
/**
************************************************************************
* @brief:      	speed_ctrl: 速度控制函数
* @param[in]:   hcan: 		指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   vel: 			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送速度控制命令
************************************************************************
**/
void speed_ctrl(CAN_HandleTypeDef*hcan,uint16_t motor_id, float vel)
{
	uint8_t *vbuf;
	vbuf=(uint8_t*)&vel;

	tx_meessage.StdId=motor_id;
	tx_meessage.IDE=CAN_ID_STD;
	tx_meessage.RTR=CAN_RTR_DATA;
	tx_meessage.DLC=0x04;

	can_send_data[0] = *vbuf;
	can_send_data[1] = *(vbuf+1);
	can_send_data[2] = *(vbuf+2);
	can_send_data[3] = *(vbuf+3);
	
		//寻空邮箱发送数据
		if(HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) 
		{
			if(HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX1) != HAL_OK)
			{
				HAL_CAN_AddTxMessage(hcan, &tx_meessage, can_send_data, (uint32_t*)CAN_TX_MAILBOX2);
			}
		}
}


